irPort = 1
cpPort = 2
motorPortL = 1
motorPortR = 3
nxt.I2Cversion    = string.char( 0x10, 0x00 )
nxt.I2Cproduct    = string.char( 0x10, 0x08 )
nxt.I2Ctype       = string.char( 0x10, 0x10 )
nxt.I2Ccontinuous = string.char( 0x10, 0x41, 0x02 )
nxt.I2Cdata       = string.char( 0x10, 0x42 )

function waitI2C( port )
  while( 0 ~= nxt.I2CGetStatus( port ) ) do
  end
end

function setupI2C(port)
  nxt.InputSetType(port,2)
  nxt.InputSetDir(port,1,1)
  nxt.InputSetState(port,1,1)
  nxt.I2CInitPins(port)
end

function IRHeading(port)
	nxt.I2CSendData(port, string.char( 0x10, 0x42 ), 8)
	waitI2C(port)
	return string.byte(nxt.I2CRecvData(port))
end
function compHeading(port)
	nxt.I2CSendData(port,  string.char( 0x02, 0x42 ), 4)
	waitI2C(port)
	data = nxt.I2CRecvData(port, 4)
	b1, b2, b3, b4 = string.byte(data, 1, 8) 
	if b4 == 1 then
		truevalue = b3 + 255
	else
		truevalue = b3
	end
	return truevalue
end

function degAdd(a, b)
	c = a + b
	if c >= 360 then
		c = c - 360
	end
	if c < 0 then
		c = c + 360
	end
	return c
end
function degDiff(a, b)
	c = a - b
	if c < 0 then
		c = c  * -1
	end
	if c > 180 then
		c = 360 - c
	end
	return c
end
function sleep(ms)
	endtime = nxt.TimerRead() + ms
	while nxt.TimerRead() < endtime do
	end
end
function whichSide(me, it)
    if me < it then
        if (it - me) < 180 then
            return 1
        else
            return 0
        end
    else
        if (me - it) < 180 then
            return 0
        else
            return 1
        end
    end
end

function steerForward()
	nxt.OutputSetSpeed(motorPortL, 32, 80)
	nxt.OutputSetSpeed(motorPortR, 32, 80)
end
function steerLeft()
	nxt.OutputSetSpeed(motorPortL, 32, 80)
	nxt.OutputSetSpeed(motorPortR, 32, -80)
end
function steerRight()
	nxt.OutputSetSpeed(motorPortL, 32, -80)
	nxt.OutputSetSpeed(motorPortR, 32, 80)
end
function allStop()
	nxt.OutputSetSpeed(motorPortL, 0, 0)
	nxt.OutputSetSpeed(motorPortR, 0, 0)
end

goal = 100
headTowards = 0
setupI2C(irPort)
setupI2C(cpPort)
while nxt.ButtonRead() == 0 do
	heading = compHeading(cpPort)
	ball5 = IRHeading(irPort)
	ball = (ball5 - 5)*30
	ball = degAdd(ball, 0)
	if ball == 0 then
		headTowards = 0
	else
		headTowards = 1
	end
	if (headTowards > 0)	then
		if whichSide(ball, 0) == 1 then
			steerRight()
		else
			steerLeft()
		end
	else
		steerForward()
	end
	sleep(100)
end
allStop()
